import sensor, image, time
from pyb import Servo
from pyb import UART
import math

white_threshold   = ((51, 100, -13, 5, -12, 15))#13, 49, 18, 61, 6, 47)#(76, 100, -11, 3, -76, 10)，(93, 100, -6, 22, -9, 43)
green_threshold = (46, 100, -48, -19, -25, 27)
sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_framesize(sensor.QVGA) # use QQVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
RIO=(64,6,218,220)
clock = time.clock() # Tracks FPS.
uart = UART(3, 115200, timeout_char=1000)                         # i使用给定波特率初始化
uart.init(1382400, bits=8, parity=None, stop=1, timeout_char=1000) # 使用给定参数初始化
def draw_arrow_green (blobs):
    for blob in   green_blobs :
        img.draw_cross(blob.cx(), blob.cy()) # cx, cy
def get_four_coordinate (blobs):
    lt = []
    for blob in blobs :
        lt += [ blob.cx()-50,blob.cy()]
    lens = len (lt)
    return lt,lens

def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob
def coordinate_sort(String,Lens)  :
    if Lens == 8 :
        for i in range(0,7,2) :
            if String[i]+String[i+1] > 360 :
                zuoshang = [String[i] ,String[i+1]]
                continue
            if   String[i]+String[i+1] < 360 and  String[i]+String[i+1] > 200 :
                if String[i] >200 :
                    zuoxia = [String[i] ,String[i+1]]
                if String[i] <200 :
                    youxia = [String[i] ,String[i+1]]
                continue
            if String[i]+String[i+1] < 200  :
                youshang = [String[i] ,String[i+1]]
    coordinates = [zuoxia,youxia,zuoshang,youshang]
    return  coordinates
pan_error = 0
tilt_error = 0
s = 0
while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # Take a picture and return the image.
    fps = clock.fps()
    blobs = img.find_blobs([white_threshold],x_stride=2,y_stride=1,roi=RIO)
    green_blobs =  img.find_blobs([green_threshold],x_stride=2,y_stride=1)
    if green_blobs :
       draw_arrow_green (green_blobs)
       string , Len = get_four_coordinate (green_blobs)
    if blobs:
        max_blob = find_max(blobs)
        pan_error = max_blob.cx()-12
        tilt_error = max_blob.cy()+15
        a,b,c,d = max_blob.rect()
        img.draw_rectangle([a,b,c,d]) # rect
        img.draw_cross(max_blob.cx(), max_blob.cy()) # cx, cy
        #was=pan_error%2
        buf = bytearray([(int(pan_error)-50),int(tilt_error),13,10])

    else:
         buf = bytearray([254,254,13,10])
    uart.write(buf)
    print([int(fps),(pan_error)-50,(tilt_error),string,Len])
